#ifndef MAT6X6_H
#define MAT6X6_H

/* -------------------------------------------------------------------------- *
 * File: Mat6x6.h                                                             *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Matrix.h"

namespace RSIM{
	
template<class T>class Mat3x3_;

template<class T>
class Mat6x6_:public Matrix_<T>{
	public:
		/** Allocate space. Set each element to 0.0 */
		Mat6x6_();
		
		/** Allocate space and set each element to val */
		explicit Mat6x6_(const T& val);
		
		/** Allocate space and initialize matrix from val. No bound check on val */
		explicit Mat6x6_(const T *val);
		
		/** Deep copy */
		Mat6x6_(const Mat6x6_<T>& M);
		
		/** Deep copy */
		Mat6x6_(const Matrix_<T>& M);
		
		/** */
		void copyMat3x3AtIndex(const int& index, const Mat3x3_<T>& M);
		
		void copyMat3x3AtIndex(const int& index, const T *M);
		
		void getMat3x3AtIndex(const int& index, Mat3x3_<T>& M)const;
		
		void getMat3x3AtIndex(const int& index, T *m)const;
		
		void getMat3x3AtIndex0(T *m)const;
		
		void getMat3x3AtIndex1(T *m)const;
		
		void getMat3x3AtIndex2(T *m)const;
		
		void getMat3x3AtIndex3(T *m)const;
		
		/** Copy values of M. Space must already be allocated for this matrix. */
		void copy(const Mat6x6_<T>& M);
		
		void copy(const T *ptr);
		
		void add(const Mat6x6_<T>& M);
		
		void add(const T *m);
		
		/** Deep copy */
		void operator=(const Mat6x6_<T>&);
		
		/** Deep copy. No validity check */
		void operator=(const Matrix_<T>&);

};

///////////////////////////////////////////////////////

template<class T>						
void Mat6x6_<T>::operator=(const Mat6x6_<T>& M){
	const T *m = M.Ptr();
	for(int i=0;i<36;++i)
		this->data_[i]=m[i];
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::operator=(const Matrix_<T>& M){
	if(M.getNElements()!=36){
		cout<<"\nDimension mismatch: From void Mat6x6_::operator=(const Matrix_&). Exiting...\n";
		exit(0);
	}
	const T *m = M.Ptr();
	for(int i=0;i<36;++i)
		this->data_[i]=m[i];
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::copyMat3x3AtIndex(const int& index, const Mat3x3_<T>& M){
	int startid=0;
	if(index==1)
		startid = 3;
	else if(index==2)
		startid = 18;
	else if(index==3)
		startid = 21;
	const T *m = M.Ptr();
	this->data_[startid] = m[0];	this->data_[startid+1] = m[1];    this->data_[startid+2] = m[2];
	this->data_[startid+6] = m[3];	this->data_[startid+7] = m[4];    this->data_[startid+8] = m[5];
	this->data_[startid+12] = m[6];	this->data_[startid+13] = m[7];    this->data_[startid+14] = m[8];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::copyMat3x3AtIndex(const int& index, const T *m){
	int startid=0;
	if(index==1)
		startid = 3;
	else if(index==2)
		startid = 18;
	else if(index==3)
		startid = 21;
	this->data_[startid] = m[0];		this->data_[startid+1] = m[1];    this->data_[startid+2] = m[2];
	this->data_[startid+6] = m[3];	this->data_[startid+7] = m[4];    this->data_[startid+8] = m[5];
	this->data_[startid+12] = m[6];	this->data_[startid+13] = m[7];    this->data_[startid+14] = m[8];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::getMat3x3AtIndex(const int& index, Mat3x3_<T>& M)const{
	int startid=0;
	if(index==1)
		startid = 3;
	else if(index==2)
		startid = 18;
	else if(index==3)
		startid = 21;
	T *m = M.wPtr();
	m[0] = this->data_[startid];		m[1] = this->data_[startid+1];    m[2] = this->data_[startid+2];
	m[3] = this->data_[startid+6];	m[4] = this->data_[startid+7];    m[5] = this->data_[startid+8];
	m[6] = this->data_[startid+12];	m[7] = this->data_[startid+13];    m[8] = this->data_[startid+14];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::getMat3x3AtIndex(const int& index, T *m)const{
	int startid=0;
	if(index==1)
		startid = 3;
	else if(index==2)
		startid = 18;
	else if(index==3)
		startid = 21;
	m[0] = this->data_[startid];		m[1] = this->data_[startid+1];    m[2] = this->data_[startid+2];
	m[3] = this->data_[startid+6];	m[4] = this->data_[startid+7];    m[5] = this->data_[startid+8];
	m[6] = this->data_[startid+12];	m[7] = this->data_[startid+13];    m[8] = this->data_[startid+14];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::getMat3x3AtIndex3(T *m)const{
	m[0] = this->data_[21];	m[1] = this->data_[22];    m[2] = this->data_[23];
	m[3] = this->data_[27];	m[4] = this->data_[28];    m[5] = this->data_[29];
	m[6] = this->data_[33];	m[7] = this->data_[34];    m[8] = this->data_[35];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::getMat3x3AtIndex2(T *m)const{
	m[0] = this->data_[18];	m[1] = this->data_[19];    m[2] = this->data_[20];
	m[3] = this->data_[24];	m[4] = this->data_[25];    m[5] = this->data_[26];
	m[6] = this->data_[30];	m[7] = this->data_[31];    m[8] = this->data_[32];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::getMat3x3AtIndex1(T *m)const{
	m[0] = this->data_[3];	m[1] = this->data_[4];    m[2] = this->data_[5];
	m[3] = this->data_[9];	m[4] = this->data_[10];    m[5] = this->data_[11];
	m[6] = this->data_[15];	m[7] = this->data_[16];    m[8] = this->data_[17];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::getMat3x3AtIndex0(T *m)const{
	m[0] = this->data_[0];	m[1] = this->data_[1];    m[2] = this->data_[2];
	m[3] = this->data_[6];	m[4] = this->data_[7];    m[5] = this->data_[8];
	m[6] = this->data_[12];	m[7] = this->data_[13];   m[8] = this->data_[14];	
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::copy(const Mat6x6_& M){
	const T *ptr = M.Ptr();
	for(int i=0;i<36;++i)
		this->data_[i]=ptr[i];
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::copy(const T *ptr){
	for(int i=0;i<36;++i)
		this->data_[i]=ptr[i];
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::add(const Mat6x6_& M){
	const T *ptr = M.Ptr();
	for(int i=0;i<36;++i)
		this->data_[i] += ptr[i];
}

///////////////////////////////////////////////////////

template<class T>
void Mat6x6_<T>::add(const T *m){
	for(int i=0;i<36;++i)
		this->data_[i] += m[i];
}

///////////////////////////////////////////////////////

template<class T>
Mat6x6_<T>::Mat6x6_():Matrix_<T>(6,6){}

///////////////////////////////////////////////////////

template<class T>
Mat6x6_<T>::Mat6x6_(const T& val):Matrix_<T>(6,6,val){}

///////////////////////////////////////////////////////

template<class T>
Mat6x6_<T>::Mat6x6_(const T *val):Matrix_<T>(6,6,val){}

///////////////////////////////////////////////////////

template<class T>
Mat6x6_<T>::Mat6x6_(const Mat6x6_<T>& M):Matrix_<T>(M,6,6){}

///////////////////////////////////////////////////////

template<class T>
Mat6x6_<T>::Mat6x6_(const Matrix_<T>& M):Matrix_<T>(M){}

///////////////////////////////////////////////////////
} // namespace RSIM



#endif
